/*! \file ScanPorts.cpp

 For information about objects in this file, see the appropriate header:
 \ref ScanPorts.h

 \section FileCopyright Copyright Notice
 Copyright (C) Xsens Technologies B.V., 2006.  All rights reserved.

 This source code is intended for use only by Xsens Technologies BV and
 those that have explicit written permission to use it from
 Xsens Technologies BV.

 THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
 KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
 IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
 PARTICULAR PURPOSE.

 \section FileChangelog	Changelog
 \par 2006-06-08, v0.0.1
 \li Job Mulder:	Created
 \par 2006-07-21, v0.1.0
 \li Job Mulder:	Updated file for release 0.1.0
 */

#ifdef _WIN32
#	include <windows.h>
#	include <string.h>
#	include <setupapi.h>
#	include <devguid.h>
#	include <regstr.h>
#else
#	include <stdlib.h>
#	include <string.h>
#	include <dirent.h>
#endif

#include <xsens_driver/cmt3.h>
#include <xsens_driver/cmtscan.h>
#include <xsens_driver/xsens_janitors.h>
#include <iostream>

namespace xsens
{


#ifdef _LOG_CMT_SCAN
#	define SCANLOG		CMTLOG
#else
#	define SCANLOG(...)
#endif

	bool abortScan = false;

	bool cmtScanPort( CmtPortInfo& portInfo, uint32_t baud, uint32_t singleScanTimeout, uint32_t scanTries )
	{
		uint32_t baudrate;
		Cmt3 port;
		port.setGotoConfigTries( scanTries ? (uint16_t) scanTries : 1 );
		port.setTimeoutConfig( singleScanTimeout );
		XsensResultValue res;

		if ( baud == 0 ) baudrate = CMT_BAUD_RATE_115K2;
		else baudrate = baud;

		while ( !abortScan )
		{
			// try to connect at current baudrate
#ifdef _WIN32
			if ((res = port.openPort(portInfo.m_portNr,baudrate)) == XRV_OK)
#else
			if ( ( res = port.openPort( portInfo.m_portName, baudrate ) ) == XRV_OK )
#endif
			{
				SCANLOG("SP: L3 port-check returns OK\n");
				portInfo.m_baudrate = baudrate;
				portInfo.m_deviceId = port.getMasterId();
				return true; // this also closes the port
			}
			// failed, determine if we need to scan other baudrates or not
			if ( res != XRV_TIMEOUT && res != XRV_TIMEOUTNODATA && res != XRV_CONFIGCHECKFAIL )
			{
				SCANLOG("SP: L3 port-check returned ERROR, aborting\n");
				return false;
			}

			SCANLOG("SP: L3 port-check returned TIMEOUT, check next baudrate or abort\n");
			// not detected, try next baudrate
			if ( baud != 0 ) return false;
			switch ( baudrate )
			{
			default:
			case CMT_BAUD_RATE_115K2:
				baudrate = CMT_BAUD_RATE_921K6;
				break;
			case CMT_BAUD_RATE_921K6:
				baudrate = CMT_BAUD_RATE_460K8;
				break;
			case CMT_BAUD_RATE_460K8:
				baudrate = CMT_BAUD_RATE_230K4;
				break;
			case CMT_BAUD_RATE_230K4:
				baudrate = CMT_BAUD_RATE_57K6;
				break;
			case CMT_BAUD_RATE_57K6:
				baudrate = CMT_BAUD_RATE_38K4;
				break;
			case CMT_BAUD_RATE_38K4:
				baudrate = CMT_BAUD_RATE_19K2;
				break;
			case CMT_BAUD_RATE_19K2:
				baudrate = CMT_BAUD_RATE_9600;
				break;
			case CMT_BAUD_RATE_9600:
				return false; // could not detect Xsens sensor, return false
			}
		}
		return false;
	}

	bool cmtScanPorts( List<CmtPortInfo>& ports, uint32_t baudrate, uint32_t singleScanTimeout, uint32_t scanTries )
	{
		CmtPortInfo current = { 0, 0, 0, "" };
		ports.clear(); // clear the list
#ifdef _WIN32
		HDEVINFO hDevInfo;
		SP_DEVINFO_DATA DeviceInfoData;
		DWORD i;

		// Create a HDEVINFO with all present devices.

		// GUID for Ports: 4D36E978-E325-11CE-BFC1-08002BE10318
		GUID portGuid =
		{	0x4D36E978,0xE325,0x11CE,
			{	0xBF,0xC1,0x08,0x00,0x2B,0xE1,0x03,0x18}};

		//	"4D36E978-E325-11CE-BFC1-08002BE10318"
		hDevInfo = SetupDiGetClassDevs(&portGuid, 0, 0, DIGCF_PRESENT | DIGCF_PROFILE);

		if (hDevInfo == INVALID_HANDLE_VALUE)
		return false;

		// Enumerate through all devices in Set.
		DeviceInfoData.cbSize = sizeof(SP_DEVINFO_DATA);
		for (i=0;!abortScan && SetupDiEnumDeviceInfo(hDevInfo,i,&DeviceInfoData);++i)
		{
			DWORD DataT;
			char buffer[256];
			bool isBT = false;

			//
			// Call function with null to begin with,
			// then use the returned buffer size
			// to Alloc the buffer. Keep calling until
			// success or an unknown failure.
			//
#if 1
			if (SetupDiGetDeviceRegistryProperty(hDevInfo,
							&DeviceInfoData,
							SPDRP_MFG,
							&DataT,
							(PBYTE)buffer,
							256,
							NULL))
			{
				// on failure, this is not an Xsens Device
				// on success, we need to check if the device is an Xsens Device
				//if (_strnicmp(buffer,"xsens",5))
				//	scan = true;
				//else
				if (!_strnicmp(buffer,"(Standard port types)",20))
				continue;
				if (_strnicmp(buffer,"xsens",5)) // if this is NOT an xsens device, treat it as a BT device

				{
					isBT = true;
					if (_strnicmp(buffer,"WIDCOMM",7)) // if this is NOT a WIDCOMM (Ezureo / TDK stack), skip it
					continue;
				}
			}
#endif
			// we found an Xsens Device, add its port nr to the list
			//Get the registry key which stores the ports settings
			HKEY hDeviceKey = SetupDiOpenDevRegKey(hDevInfo, &DeviceInfoData, DICS_FLAG_GLOBAL, 0, DIREG_DEV, KEY_QUERY_VALUE);
			if (hDeviceKey != INVALID_HANDLE_VALUE)
			{
				//Read in the name of the port
				char pszPortName[256];
				DWORD dwSize = 256;
				DWORD dwType = 0;
				if ((RegQueryValueEx(hDeviceKey, "PortName", NULL, &dwType, (LPBYTE) pszPortName, &dwSize) == ERROR_SUCCESS) && (dwType == REG_SZ))
				{
					//If it looks like "COMX" then
					//add it to the array which will be returned
					int32_t nLen = (int32_t) strlen(pszPortName);
					if (nLen > 3)
					{
						if (_strnicmp(pszPortName, "COM", 3))
						continue;
						int32_t nPort = atoi(&pszPortName[3]);
						if (nPort)
						{
							current.m_portNr = (uint16_t) nPort;
							if (isBT)
							current.m_baudrate = CMT_BAUD_RATE_460K8;
							else
							current.m_baudrate = baudrate;
							ports.append(current);
						}
					}
				}
			}
			//Close the key now that we are finished with it
			RegCloseKey(hDeviceKey);
		}

		//  Cleanup

		SetupDiDestroyDeviceInfoList(hDevInfo);

		// Now sort the list by ascending port nr
		ports.sortAscending();

		// Add the standard com ports 1 and 2 unless they are already in the list
		bool add1 = true,
		add2 = true;
		if ((ports.length() > 0) && (ports[0].m_portNr == 1))
		add1 = false;
		if (ports.length() > 0)
		{
			if (ports[0].m_portNr == 2)
			add2 = false;
			else
			if (ports.length() > 1)
			if (ports[1].m_portNr == 2)
			add2 = false;
		}
		if (add1)
		{
			current.m_portNr = 1;
			current.m_baudrate = baudrate;
			ports.append(current);
		}
		if (add2)
		{
			current.m_portNr = 2;
			current.m_baudrate = baudrate;
			ports.append(current);
		}
#else
		DIR *dir;
		struct dirent *entry;

		if ( ( dir = opendir( "/dev/" ) ) == NULL ) return false;

		while ( ( entry = readdir( dir ) ) )
			if ( strncmp( "ttyS", entry->d_name, 4 ) == 0 || ( strncmp( "ttyUSB", entry->d_name, 6 ) == 0 /*&& strncmp( "ttyUSB0", entry->d_name, 7 ) != 0*/ ) )
			{
				std::cout << "scanning /dev/" << entry->d_name << std::endl;
				sprintf( current.m_portName, "/dev/%s", entry->d_name );
				current.m_baudrate = baudrate;
				ports.append( current );
			}
		closedir( dir );

		ports.sortAscending();
#endif


		// try to connect so we can detect if there really is an MT / XM attached
		unsigned p = 0;
		while ( !abortScan && p < ports.length() )
		{
			if ( cmtScanPort( ports[p], ports[p].m_baudrate, singleScanTimeout, scanTries ) ) ++p;
			else ports.remove( p );
		}

		if ( abortScan ) return abortScan = false;

		// Now sort the final list by ascending port nr
		ports.sortAscending();
		abortScan = false;
		return true;
	}

} // end of xsens namespace
